1. Field of the Invention
The present invention relates to a robot speed computing apparatus and a robot speed computing method for use in a robot locus control apparatus or a robot locus simulation apparatus. More particularly, the present invention relates to a robot speed computing apparatus and a robot speed computing method which permit a robot to pass through the singular area of speed without excessively increasing the speed when linear interpolation is effected in rectangular coordinates.
2. Description of the Related Art
Linear-interpolation-based locus control in the field of robot control is a method of controlling the speed of a robot such that the locus of the front end of the robot becomes linear, and that the speed of the robot follows a trapezoidal speed pattern, or the like.
With reference to FIGS. 19 and 20, a conventional linear-interpolation-based locus control method for a robot will be described.
FIG. 19 is a flowchart of a conventional linear interpolation algorithm, and FIG. 20 is a plot showing the state of the speed of the front end of the robot obtained when the robot is subjected to conventional linear-interpolation-based locus control.
In FIG. 19, linear interpolation is started from step 101. The linear interpolation procedure is started when it is invoked by the procedure of a higher-level work program. When the linear interpolation procedure is invoked, the current position and a final target position are determined by the procedure of a higher-level work program. Then, the linear interpolation procedure is given the thus-determined positions.
When the linear interpolation is started, a travel distance is initially calculated from a starting position and the final target position of the robot in step 102. In step 103, a remaining travel distance over which the robot must travel is calculated from the current position and final target position of the robot.
A designated speed v1 is calculated by multiplying together a designated speed included in a program, a speed multiplying factor designated in the program, and a multiplying factor designated from a control panel in step 104. The increment of speed obtained by multiplying a designated acceleration .alpha.1 by a sampling time .DELTA.t is added to a current speed v0, whereby a speed v2 associated with an acceleration pattern operation is calculated from the following equation 1: EQU v2=v0+.alpha.1.times..DELTA.t (1)
A speed at which the robot travels over the remaining distance is calculated in step 103 according to a deceleration pattern so as to decelerate at a constant rate by the deceleration designated in the program. More specifically, a speed v3, at which the robot continues decelerating at a designated deceleration .alpha.2 and decelerates to a speed of zero at the end of the travel over only the remaining distance L, is calculated from the following equation 2. EQU v3=.sqroot. 2.times..alpha.2.times.L (2)
Of the three speeds calculated in steps 104, 105, and 106, the minimum speed is adopted as an instruction speed in step 130.
For example, the instruction speed is more specifically described with reference to FIG. 20 which shows the state of the speed of the front end of the robot. At time t=t1, the speed v2 associated with the acceleration pattern operation calculated in step 105 is the minimum speed, and hence this speed is adopted as the instruction speed. In contrast, at time t=t2, the designated speed v1 which is calculated in step 104 and is a constant velocity is the minimum speed. Therefore, this speed is adopted. Further, at time t=t3, the speed v3 associated with the deceleration pattern operation calculated in step 106 is the minimum. Therefore, this speed is adopted.
Turning again to FIG. 19, interpolation positions over which the robot travels at the instruction speed determined in step 130 are calculated from the current position thereof in step 109. Subsequently, the interpolation positions represented in the form of rectangular coordinates are converted into joint coordinates in step 110. A joint of the robot is moved to the target interpolation position in step 111. The current position is updated to the interpolation positions in step 112.
The current position is compared with the final target position in step 113, whereby it is decided whether or not the robot has reached the target position. If the robot has not reached the final target position, processing will proceed to step 103. In contrast, if the robot has reached the final target position, the procedure of linear interpolation is completed.
However, according to the conventional linear-interpolation-based locus control of a robot, linear interpolation is effected by considering only the speed of the front end of the robot, as previously described. For example, in the case of a scalar robot, the speed of a first shaft becomes a considerably large value regardless of whether the speed of the front end of the robot is high or low. Therefore, it is very difficult for the robot to pass the vicinity of the point of origin during the course of its linear interpolation operations.
The aforementioned problem will be described in more detail with reference to a diagram showing operations of a robot in the prior art when it passes through a singular area shown in FIG. 21.
FIG. 21 shows a scalar robot which linearly interpolates positions (1) to (4) by passing through the point of origin along the X axis. In the drawing, the scalar robot has a first arm Al which is 5 cm long and a second arm A2 which is also 5 cm long. The front end of the second arm A2 travels leftwards along the X axis at a rate of 5 cm per unit time, passing sequentially the positions (1), (2), (3), and (4).
In the case shown in FIG. 20, the first arm A1 rotates counterclockwise through 58.degree. when it travels from the position (1) to the position (2). It rotates counterclockwise through 117.degree. when traveling from the position (2) to (3). Further, it rotates clockwise through 16.degree. when traveling from the position (3) to the position (4).
In the linear interpolation operations, the speed of the front end of the robot; namely, the speed of the front end of the second arm A2 shown in FIG. 21, is constant. Therefore, the first arm A1 must move through a considerably large angle when it passes through the vicinity of the point of origin during the course of the travel from the position (2) to the position (3). In the case shown in FIG. 21, the first arm A1 must move from the position (2) to the position (3) at about twice the angular velocity as when it travels from the position (1) to the position (2) and at about seven times the angular velocity as when it travels from the position (3) to the position (4).
For these reasons, it becomes necessary for a first axis to rotate at a fast rotating speed in excess of the capability of the first axis, depending on conditions, thereby leading to an excessive speed error. As a result, the operation of the robot is interrupted.
This excessive speed error will be described with reference to FIG. 22. FIG. 22 shows the state of speed, in which the vertical axis is the speed of the front end of the robot and the rotational angular velocity of a first joint shaft, whereas the horizontal axis is time. In FIG. 22, a trapezoidal diagram B1 represents the speed of the front end of the robot obtained in the manner as shown in FIG. 20, and a concave function curve B2 represents the rotational angular velocity of the first joint. As the front end of the robot travels, the first joint moves. The rotational angular velocity becomes gradually increased. If the robot moves through such a large angle as in the case of the movement from the position (2) to the position (3) shown in FIG. 21, the rotational angular velocity of the first joint will exceed the maximum rated angular velocity J1max of the first joint (this maximum rated angular velocity was exceeded at t=t0 in FIG. 22). An excessive speed error arises in the robot, which makes it impossible for the robot to continue the subsequent operations.
The above-described problem will be mathematically proven.
In the case of a scalar robot, provided that the position of the front end of the robot is (X, Y), and that a joint angle (.theta.1, .theta.2), the relationship between the speed of the front end and the angular speed of the joint can be expressed as the following equation (3) by use of a Jacobian matrix J. ##EQU1##
In this expression, J represents a Jacobian matrix as defined in the form of the following expression (4). ##EQU2##
From the speed of the front end, the following expression (5) is established with regard to the angular velocity of the joint by means of an inverse Jacobian matrix Jr. ##EQU3##
In this expression, Jr is an inverse Jacobian matrix as defined in the form of the following matrix 6. ##EQU4##
The above expression (6) is a simultaneous equation with two unknowns. The first expression of the simultaneous equation represents the relationship between the angular velocity of the joint of the first shaft and the speed of the front end. In the case of a two-degree-of-freedom scalar robot shown in FIG. 21, the robot has only a component of velocity in the X direction, and a component of velocity in the Y direction of the robot becomes zero. Therefore, the angular velocity J1 of a joint of the first axis is expressed as the following expression (7). ##EQU5##
According to the above-described expression (7), the front end of the second arm A2 approaches the point of origin, and the first arm A1 and the second arm A2 overlap each other; namely, as .theta.2=.pi. approaches, the denominator of J1 approaches zero. Therefore, the angular velocity of a joint of the first shaft approaches infinity irrespective of the value of the speed dX/dt of the front end of the robot.
Therefore, it becomes necessary for the first shaft to rotate at a higher rotating speed in excess of the capability of the first shaft, thereby resulting in an excessive speed error. This error may lead to interruption of the operation of the robot.
Similarly, in the case of a cylindrical robot shown in FIG. 10 and a scalar robot shown in FIG. 4 which will be described later, the relationship between the speed of the front end of the robot and the speed of the joint can be expressed as the following expression (8) by means of the Jacobian matrix J. ##EQU6##
Here, J is a Jacobian matrix defined as the following expression (9). ##EQU7##
Therefore, from the speed of the front end of the robot, the following expression (10) will be established with regard to the speed of the joint by means of an inverse Jacobian matrix Jr. ##EQU8##
Here, Jr designates an inverse Jacobian matrix which is defined as the following expression (11). ##EQU9##
If the robot moves along the X axis as in the case shown in FIG. 21, the relationship between the speeds of the front end of the robot dx/dt and d.theta./dt is defined as the following expression (12) from the second expression of the previously-described expression (6) which is a simultaneous equation. ##EQU10##
If the number of rotations of a pivot reaches the maximum number at a predetermined speed of the front end dx/dt in the vicinity of .theta.=45, the pivot must rotate at about 1.41 times its maximum number of rotations as obtained by sin (90.degree.)/sin (45.degree.)=21/2.noteq.1.41. In other words, the pivot rotation must increase by 40% over its maximum number rotational speed. As a result, the speed of the pivot becomes excessive. Conversely, if the robot is set so as to prevent its speed becoming excessive, the robot will have redundant capability while being positioned during the remaining course of operation. An effective value of a sine curve in the overall working area becomes 2/.pi.=0.63, and therefore about 37% of the capability of the robot is not utilized.
To solve the aforementioned problem, it has been proposed to apply a feedback technique to the linear interpolation based on a conventional locus control method for a robot, as disclosed in, e.g., Unexamined Japanese Patent Application No. Sho-58-114888.
FIG. 23 shows a flowchart which represents a control method disclosed in Unexamined Japanese Patent Application No. Sho-58-114888. With reference to FIG. 23, a proposed locus control method of the above-described patent application by which an excessive speed error is prevented from arising in a singular area, will be described.
Processing carried out in steps 101 to 110 of the flowchart is the same as the processing carried out in steps of the flowchart of the basic control method shown in FIG. 19.
It is decided in step 131 whether or not the speed of the joint has become excessive in coordinates of a joint. If the speed of any one of joints has not become excessive, the processing following step 111 is carried out as in the case of the basic control method shown in FIG. 19. In contrast, if the speed of any one of the joints has become excessive, processing will then proceed to step 132, and an instruction of speed will be controlled. More specifically, a rate of excess between the angular velocity of the shaft whose speed has come to exceed the maximum speed is obtained in step 131. Subsequently, the instruction speed is controlled so as to prevent the speed of the shaft from becoming excessive by multiplying together the instruction speed and the inverse of the rate of excess.
For instance, assume that the first joint having a maximum angular velocity of 3.14 rad/s is given 4.72 rad/s as an instruction because of an instruction speed of 90 mm/s as a result of transformation of a coordinate system. The rate of excess of the first joint is 4.72 rad/s.div.3.14 rad/s=1.5 times. Therefore, the instruction speed is controlled by multiplying it by the inverse of that result, i.e., 0.667, so that the instruction speed becomes 60 mm/s.
The processing carried out in steps 111 to 114 of the flowchart is the same as the processing carried out in the flowchart of the conventional basic control method shown in FIG. 19.
The proposed method disclosed in Unexamined Japanese Patent Application No. Sho-58-114888 uses the feedback technique as previously described. For this reason, an instruction for instructing the joint to exceed the maximum rated angular velocity should be generated by executing a sequence of operations in steps 104 to 110. The result of these operations should be discarded once, and the operations in steps 109 to 110 must be carried out at least one more time. In the two-degree-of-freedom robot, the transformation of a coordinate system carried out in step 110 involves execution of an inverse tangent function one time, calculation of a square root one time, squaring operations six times, calculation of a fourth power twice, and additions and subtractions six times. In a robot speed computing apparatus, a sampling interval is usually set to as short a period of time as possible in order to increase the accuracy of a locus. The sampling time is not set so as to allow execution of these complicated arithmetic operations more than twice.
For these reasons, an instruction is not assigned to the joint if the arithmetic operations are not completed within the period of the sampling time. As a result, the locus of the robot becomes jumbled, or alternatively the robot must be provided with a sophisticated CPU 11 capable of executing the operations in step 110 more than twice within a period of the sampling time.
Unexamined Japanese Patent Application No. Sho-59-163609, Unexamined Japanese Patent Application No. Sho-62-189504, Unexamined Japanese Patent Application No. Hei-2-47702, Unexamined Japanese Patent Application No. Hei-3-66576, Unexamined Japanese Patent Application No. Hei-6-324730, and Unexamined Japanese Patent Application No. Hei-7-72910 disclose methods that use the same feedback technique as used in the method disclosed in Unexamined Japanese Patent Application No. Sho-58-114888. Since these methods use the feedback technique, the locus of the robot becomes jumbled as does the robot disclosed in Unexamined Japanese Patent Application No. Sho-58-114888.
A robot control method is proposed in Unexamined Japanese Patent Application No. Hei-2-308311. According to this control method, all the interpolation points which exist between the starting point to the target point are previously calculated before the movement of the robot is started in order to prevent interruption of an operation of the robot due to excessive speed errors. However, according to this method, it is necessary to previously calculate all the interpolation points. Because of this, it is necessary to stop the operation of the robot in order to calculate the interpolation points before starting the operation, or alternatively a high-speed processor is required to avoid interruption of the operation of the robot. Further, according to this method, it is impossible to change the speed of an operation from an external control panel by use of a speed change instruction during the course of the linear operation.
Unexamined Japanese Patent Application No. Sho-60-57408 discloses a robot control method. According to this method, an instruction speed is invalidated only for the period of time during which the robot passes a singular area. The robot is operated at a predetermined slow speed while passing the singular area so as to prevent interruption of the operation of the robot due to an excessive speed error. According to this method, the robot is operated at such a slow speed as not to bring about an excessive speed while passing through all the positions within the singular area in spite of the fact that the robot is capable of actuating at a much higher speed. As a result, the speed of the robot becomes very slow.
A robot control method is disclosed in Unexamined Japanese Patent Application Nos. Hei-3-66576 and Hei-6-170765. According to this method, an instruction speed is reduced by multiplying it by a predetermined coefficient less than one so as to prevent interruption of an operation of the robot due to an excessive speed error, as required, when a robot enters a singular area. However, this robot control method, fails to disclose a method of determining a coefficient used in reducing the speed in order prevent an excessive speed. In practice, it is necessary for an operator to determine a coefficient of each robot for each path by trial and error.